/**
 * Copyright 2013 Douglas Lacher
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * @author Douglas Lacher <doug.lacher@gmail.com>
 */

package com.devioustoast.andmoco;

import java.io.File;
import java.io.FileInputStream;
import java.io.IOException;
import java.io.InputStreamReader;
import java.util.HashMap;
import java.util.Map;

import org.xmlpull.v1.XmlPullParserException;

import android.content.SharedPreferences;
import android.content.res.AssetManager;
import android.util.Log;
import android.util.SparseArray;
import android.util.SparseIntArray;

/**
 * A Robot represents the hexapod that's being driven by this application.  This class contains
 * movements and manages the robot's state and connectivity.
 *
 * @author Douglas Lacher <doug.lacher@gmail.com>
 */
public class Robot {

  public static final String LOG_TAG = "Robot";
  public static final int SERVO_COUNT = 32;
  public static final int NECK_SERVO = 31;

  // PoMoCo commands from https://github.com/ArcBotics/Hexy/wiki/Servotor32.
  public static final String POMOCO_COMMAND_VERSION = "V";
  public static final String POMOCO_COMMAND_CENTER_ALL = "C";
  public static final String POMOCO_COMMAND_KILL_ALL = "K";
  public static final String POMOCO_COMMAND_PRINT_DEBUG = "D";

  public final Leg leftFront;
  public final Leg leftMiddle;
  public final Leg leftBack;
  public final Leg rightFront;
  public final Leg rightMiddle;
  public final Leg rightBack;
  public final Servo neck;

  // This template requires the servo number to be substituted for the placeholder.
  private static final String SERVO_PREFERENCE_NAME_TEMPLATE = "servo_offset_%s";

  private BluetoothConnector mBluetoothConnector;
  private SharedPreferences mSharedPreferences;
  private SparseIntArray mOffsetMap;
  private SparseArray<Servo> mServoMap;
  private Map<String, Runnable> mMoves;
  private Map<String, Runnable> mOperations;

  /**
   * {@link Robot#scanMovesSourcesAndBuildMovesMap(File, AssetManager)} must be called after
   * constructing a new Robot and before sending the Robot any moves.
   *
   * @param bluetoothConnector a BluetoothConnector to use to communicate with the robot.
   * @param preferences a SharedPreferences from which the offset preferences can be read.
   * @param externalStorageMoves the directory from which to read the moves files.  If the
   *        directory does not exist or is not readable, then /<app-root>/assets/moves will be
   *        used from the asset manager.
   * @param assetManager an AssetManager from the parent Activity.
   */
  public Robot(BluetoothConnector bluetoothConnector, SharedPreferences preferences) {
    mBluetoothConnector = bluetoothConnector;
    mSharedPreferences = preferences;
    mOffsetMap = buildOffsetMap(preferences);
    mServoMap = buildServoMap(mBluetoothConnector, mOffsetMap);
    mOperations = buildOperationsMap();

    leftFront = new Leg(LegName.LF, mServoMap.get(7), mServoMap.get(6), mServoMap.get(5));
    leftMiddle = new Leg(LegName.LM, mServoMap.get(11), mServoMap.get(10), mServoMap.get(9));
    leftBack = new Leg(LegName.LB, mServoMap.get(15), mServoMap.get(14), mServoMap.get(13));
    rightFront = new Leg(LegName.RF, mServoMap.get(24), mServoMap.get(25), mServoMap.get(26));
    rightMiddle = new Leg(LegName.RM, mServoMap.get(20), mServoMap.get(21), mServoMap.get(22));
    rightBack = new Leg(LegName.RB, mServoMap.get(16), mServoMap.get(17), mServoMap.get(18));
    neck = mServoMap.get(NECK_SERVO);
  }

  /**
   * Builds the Robot's moves from the provided directory.
   *
   * If the directory doesn't exist, is unreadable, or is empty, then moves will be read from
   * the asset manager.
   *
   * @param externalStorageMoves the directory from which to read the moves files.  If the
   *        directory does not exist or is not readable, then assetsPath will be
   *        used from the asset manager.
   * @param assetManager an AssetManager from the parent Activity.
   * @param assetsPath the path within the assets directory from which to read moves.
   */
  public void scanMovesSourcesAndBuildMoves(
      File externalStorageMoves, AssetManager assetManager, String assetsPath) {
    if (externalStorageMoves.exists() && externalStorageMoves.canRead()) {
      String[] files = externalStorageMoves.list();
      if (files.length > 0) {
        mMoves = buildMovesMapFromDirectory(externalStorageMoves, files);
      } else {
        mMoves = buildMovesMapFromAssets(assetManager, assetsPath);
      }
    } else {
      mMoves = buildMovesMapFromAssets(assetManager, assetsPath);
    }
  }

  public Map<String, Runnable> getMovesMap() {
    return mMoves;
  }

  public Map<String, Runnable> getOperationsMap() {
    return mOperations;
  }

  public SparseArray<Servo> getServoMap() {
    return mServoMap;
  }

  public Leg getLeg(LegName legName) {
    Leg leg = null;
    switch (legName) {
    case LF:
      leg = leftFront;
      break;
    case LM:
      leg = leftMiddle;
      break;
    case LB:
      leg = leftBack;
      break;
    case RF:
      leg = rightFront;
      break;
    case RM:
      leg = rightMiddle;
      break;
    case RB:
      leg = rightBack;
      break;
    default:
      // No default.
      break;
    }
    return leg;
  }

  /**
   * Centers all servos.
   *
   * @throws InterruptedException if the data could not be added to the send queue.
   */
  public void centerAllServos() throws InterruptedException {
    Log.d(LOG_TAG, "Centering all servos.");
    mBluetoothConnector.send(POMOCO_COMMAND_CENTER_ALL);
  }

  /**
   * Kills all servos.
   *
   * @throws InterruptedException if the data could not be added to the send queue.
   */
  public void killAllServos() throws InterruptedException {
    Log.d(LOG_TAG, "Killing all servos.");
    mBluetoothConnector.send(POMOCO_COMMAND_KILL_ALL);
  }

  /**
   * Kills a specific servo.
   *
   * @param servoNumber the servo number to kill.
   * @throws InterruptedException if the data could not be added to the send queue.
   */
  public void killServo(int servoNumber) throws InterruptedException {
    mServoMap.get(servoNumber).killServo();
  }

  /**
   * Moves a specific servo to the new position.
   *
   * @param servoNumber the servo number.
   * @param microseconds the relative position, specified in microseconds (e.g. -20 or 40).
   * @throws InterruptedException if the data could not be added to the send queue.
   */
  public void moveServo(int servoNumber, int position) throws InterruptedException {
    mServoMap.get(servoNumber).setRelativePositionUsec(position);
  }

  /**
   * Saves the servo offsets back to the SharedPreferences.
   */
  public void saveOffsetMap() {
    SharedPreferences.Editor editor = mSharedPreferences.edit();
    for (int i = 0; i < mServoMap.size(); i++) {
      editor.putInt(String.format(SERVO_PREFERENCE_NAME_TEMPLATE, i), mServoMap.get(i).getOffset());
    }
    editor.commit();
  }

  /**
   * Builds a map of servo offsets from the saved preferences.  If no offsets are saved for a servo,
   * then that servo defaults to 0.
   *
   * @param preferences a SharedPreferences from which the offset preferences can be read.
   * @return a SpareIntArray of the offsets indexed by servo number.
   */
  private SparseIntArray buildOffsetMap(SharedPreferences preferences) {
    SparseIntArray offsetMap = new SparseIntArray(SERVO_COUNT);
    for (int i = 0; i < SERVO_COUNT; i++) {
      offsetMap.put(i, preferences.getInt(String.format(SERVO_PREFERENCE_NAME_TEMPLATE, i), 0));
    }
    return offsetMap;
  }

  /**
   * Builds a map of 32 Servo objects, indexed by their number, and returns the map.
   *
   * @param bluetoothConnector the BluetoothConnector to use for communicating with each Servo.
   * @param offsets a map of the servo's offsets keyed by servo number.
   * @return an array of Servos, indexed by their number.
   */
  private SparseArray<Servo> buildServoMap(
      BluetoothConnector bluetoothConnector, SparseIntArray offsets) {
    SparseArray<Servo> servoMap = new SparseArray<Servo>(SERVO_COUNT);
    for (int i = 0; i < SERVO_COUNT; i++) {
      servoMap.put(i, new Servo(bluetoothConnector, i, offsets.get(i)));
    }
    return servoMap;
  }

  /**
   * Builds a Map of Moves from files located at MOVES_PATH.
   *
   * @param assetManager an AssetManager from which the move files can be read.
   * @param assetsPath the path within the assets directory from which to read moves.
   * @return a Map of Moves keyed by name.
   */
  private Map<String, Runnable> buildMovesMapFromAssets(
      AssetManager assetManager, String assetsPath) {
    Map<String, Runnable> moveMap = new HashMap<String, Runnable>();
    Move parsedMove;
    String[] assetList = new String[] {};
    try {
      assetList = assetManager.list(assetsPath);
      for(String asset : assetList) {
        if (asset.endsWith(".xml")) {
          InputStreamReader reader = null;
          try {
            String assetPath = String.format("%s/%s", assetsPath, asset);
            Log.d(LOG_TAG, String.format("Reading move file %s.", assetPath));
            parsedMove = MoveParser.parseMoveFile(assetManager.open(assetPath), this);
            moveMap.put(parsedMove.name, parsedMove);
          } catch (XmlPullParserException e) {
            Log.w(LOG_TAG, e);
          } catch (IOException e) {
            Log.w(LOG_TAG, e);
          } finally {
            if (reader != null) {
              reader.close();
            }
          }
        }
      }
    } catch (IOException e) {
      Log.e(LOG_TAG, "IOException thrown while reading move files.", e);
    }
    return moveMap;
  }

  /**
   * Builds a Map of Moves from files located at MOVES_PATH.
   *
   * @param directory the directory from which to read the files.
   * @param moves an array of file names to read from the directory.
   * @return a Map of Moves keyed by name.
   */
  private Map<String, Runnable> buildMovesMapFromDirectory(File directory, String[] moves) {
    Map<String, Runnable> moveMap = new HashMap<String, Runnable>();
    Move parsedMove;
    try {
      for(String move : moves) {
        if (move.endsWith(".xml")) {
          InputStreamReader reader = null;
          try {
            String filePath = String.format("%s/%s", directory.getAbsolutePath(), move);
            Log.d(LOG_TAG, String.format("Reading move file %s.", filePath));
            parsedMove = MoveParser.parseMoveFile(new FileInputStream(filePath), this);
            moveMap.put(parsedMove.name, parsedMove);
          } catch (XmlPullParserException e) {
            Log.w(LOG_TAG, e);
          } catch (IOException e) {
            Log.w(LOG_TAG, e);
          } finally {
            if (reader != null) {
              reader.close();
            }
          }
        }
      }
    } catch (IOException e) {
      Log.e(LOG_TAG, "IOException thrown while reading move files.", e);
    }
    return moveMap;
  }

  /**
   * Builds a Map of Runnables containing the robot-wide operations that aren't a Move.
   *
   * @return a Map of Runnables keyed by name.
   */
  private Map<String, Runnable> buildOperationsMap() {
    Map<String, Runnable> operationsMap = new HashMap<String, Runnable>();
    operationsMap.put("Kill All Servos", new Runnable() {
      @Override
      public void run() {
        try {
          mBluetoothConnector.send(POMOCO_COMMAND_KILL_ALL);
        } catch (InterruptedException e) {
          Log.w(LOG_TAG, "Interrupted while killing all servos.", e);
        }
      }});
    operationsMap.put("Center All Servos", new Runnable() {
      @Override
      public void run() {
        try {
          mBluetoothConnector.send(POMOCO_COMMAND_CENTER_ALL);
        } catch (InterruptedException e) {
          Log.w(LOG_TAG, "Interrupted while centering all servos.", e);
        }
      }});
    return operationsMap;
  }
}
